{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# LAGO (Linear Approximation for Graph Optimization)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n",
        "LAGO stands for **Linear Approximation for Graph Optimization**. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n",
        "\n",
        "The core idea is:\n",
        "1.  **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n",
        "2.  **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n",
        "\n",
        "Key functions:\n",
        "*   `initializeOrientations(graph)`: Computes initial estimates for the `Rot2` (orientation) components of the poses in the graph.\n",
        "*   `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n",
        "*   `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n",
        "\n",
        "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose.\n",
        "\n",
        "**Important Note**: LAGO expects integer keys numbered from 0 to n-1, with n the number of poses."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/lago.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import gtsam.utils.plot as gtsam_plot\n",
        "import matplotlib.pyplot as plt\n",
        "import numpy as np\n",
        "from gtsam import NonlinearFactorGraph, Pose2, PriorFactorPose2, BetweenFactorPose2\n",
        "from gtsam import lago"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "example_header_md"
      },
      "source": [
        "## Example Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "example_desc_md"
      },
      "source": [
        "We'll create a simple 2D pose graph with odometry and a loop closure, then use `lago.initialize`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "id": "example_pipeline_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Initial Estimate from LAGO:\n",
            "\n",
            "Values with 5 values:\n",
            "Value 0: (gtsam::Pose2)\n",
            "(-7.47244713e-17, -6.32592437e-17, -0.00193783525)\n",
            "\n",
            "Value 1: (gtsam::Pose2)\n",
            "(1.70434147, -0.00881225307, 0.034656973)\n",
            "\n",
            "Value 2: (gtsam::Pose2)\n",
            "(3.40930145, 0.0555625509, 1.64569894)\n",
            "\n",
            "Value 3: (gtsam::Pose2)\n",
            "(2.9638596, 2.05327873, 3.10897006)\n",
            "\n",
            "Value 4: (gtsam::Pose2)\n",
            "(0.669190885, 2.11357777, -1.71695927)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# 1. Create a NonlinearFactorGraph with Pose2 factors\n",
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# Add a prior on the first pose\n",
        "prior_mean = Pose2(0.0, 0.0, 0.0)\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n",
        "graph.add(PriorFactorPose2(0, prior_mean, prior_noise))\n",
        "\n",
        "# Add odometry factors (simulating moving in a square)\n",
        "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
        "graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))\n",
        "graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
        "graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
        "graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
        "\n",
        "# Add a loop closure factor\n",
        "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n",
        "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n",
        "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n",
        "graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))\n",
        "\n",
        "# 2. Perform LAGO initialization\n",
        "initial_estimate_lago = lago.initialize(graph)\n",
        "\n",
        "print(\"\\nInitial Estimate from LAGO:\\n\")\n",
        "initial_estimate_lago.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The block below visualizes the initial estimate computed by the LAGO algorithm. It uses the `gtsam_plot.plot_pose2` function to plot the poses in 2D space. Each pose is represented with its position and orientation, providing an intuitive way to inspect the initialization results. The visualization helps verify the correctness of the initial guess before proceeding with further optimization."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/plain": [
              "<Figure size 640x480 with 1 Axes>"
            ]
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "plt.figure(1)\n",
        "for key in initial_estimate_lago.keys():\n",
        "    gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n",
        "plt.title(\"LAGO Initial Estimate\")\n",
        "plt.axis('equal')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Finally, let's look at `lago.initializeOrientations` to compute the initial orientation estimatesThis solves a linear system, and the solution is represented as a `VectorValues` object, which stores the estimated angles for each pose as a 1-d vector.\n",
        "\n",
        "We compare these orientation estimates with the orientations extracted from the full LAGO initialization (`lago.initialize`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "LAGO Orientations (VectorValues):\n",
            "VectorValues: 6 elements\n",
            "  0: -1.11022302e-16\n",
            "  1: -0.008\n",
            "  2: 1.55479633\n",
            "  3: 3.11759265\n",
            "  4: 4.68038898\n",
            "  99999999: 0\n",
            "Orientations from full LAGO Values:\n",
            " 0: -0.0019\n",
            " 1: 0.0347\n",
            " 2: 1.6457\n",
            " 3: 3.1090\n",
            " 4: -1.7170\n"
          ]
        }
      ],
      "source": [
        "initial_orientations = lago.initializeOrientations(graph)\n",
        "print(\"\\nLAGO Orientations (VectorValues):\")\n",
        "initial_orientations.print()\n",
        "\n",
        "print(\"Orientations from full LAGO Values:\")\n",
        "for i in range(5):\n",
        "    print(f\" {i}: {initial_estimate_lago.atPose2(i).theta():.4f}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "These are not as accurate (the last one is actually fine, it's $2\\pi$ off) but will still be good enough as an initial estimate."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
